'''
Author: wangyingjie 2778809626@qq.com
Date: 2025-07-03 15:35:52
LastEditors: wangyingjie 2778809626@qq.com
LastEditTime: 2025-07-04 13:50:29
FilePath: /two_wheeled_car/src/bot_gazebo/launch/obstacle_world.launch.py
Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
'''
# 该函数 generate_launch_description 的功能是：

# 1. 设置 Gazebo 所需的模型路径（`GAZEBO_MODEL_PATH`）和插件路径（`GAZEBO_PLUGIN_PATH`），确保其能找到机器人模型与插件；
# 2. 加载 Gazebo 启动文件，启动仿真环境；
# 3. 声明一个可选的启动参数 `world`，用于指定 SDF 格式的仿真世界文件，默认为 [dynamic.world](file:///home/wyj/myself/1_code/3_ros2_about/two_wheeled_car/src/bot_gazebo/worlds/dynamic.world)。


import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from ament_index_python.packages import get_package_prefix

def generate_launch_description():

    # 获取 gazebo_ros 和 bot_gazebo 包的共享目录
    pkg_gazebo_ros = get_package_share_directory('gazebo_ros')
    pkg_bot_gazebo = get_package_share_directory('bot_gazebo')
    # 获取 bot_gazebo 包中的插件目录路径
    plugin_path = os.path.join(pkg_bot_gazebo,'plugins')

    # 获取整个安装目录
    # 这样做是为了避免手动复制或软链接包以便 Gazebo 能找到它们
    description_package_name = "bot_description"
    install_dir = get_package_prefix(description_package_name)

    # 设置 WORLD 模型文件的路径。用于在 my_robot_mode 包的 models 文件夹中查找模型
    gazebo_models_path = os.path.join(pkg_bot_gazebo, 'models')
    # os.environ["GAZEBO_MODEL_PATH"] = gazebo_models_path

    # 配置 GAZEBO_MODEL_PATH 环境变量以包含模型路径
    if 'GAZEBO_MODEL_PATH' in os.environ:
        os.environ['GAZEBO_MODEL_PATH'] = os.environ['GAZEBO_MODEL_PATH'] + \
            ':' + install_dir + '/share' + ':' + gazebo_models_path
    else:
        os.environ['GAZEBO_MODEL_PATH'] = install_dir + \
            "/share" + ':' + gazebo_models_path

    # 配置 GAZEBO_PLUGIN_PATH 环境变量以包含插件路径
    os.environ['GAZEBO_PLUGIN_PATH'] = os.environ['GAZEBO_PLUGIN_PATH'] + ":" + plugin_path
    if 'GAZEBO_PLUGIN_PATH' in os.environ:
        os.environ['GAZEBO_PLUGIN_PATH'] = os.environ['GAZEBO_PLUGIN_PATH'] + \
            ':' + install_dir + '/lib'
    else:
        os.environ['GAZEBO_PLUGIN_PATH'] = install_dir + '/lib'

    # 打印已配置的环境变量
    print("GAZEBO MODELS PATH=="+str(os.environ["GAZEBO_MODEL_PATH"]))
    print("GAZEBO PLUGINS PATH=="+str(os.environ["GAZEBO_PLUGIN_PATH"]))

    # GAZEBO MODELS PATH==/usr/share/gazebo/../../share/gazebo-11/models::/home/wyj/myself/1_code/3_ros2_about/two_wheeled_car/install/bot_description/share:/home/wyj/myself/1_code/3_ros2_about/two_wheeled_car/install/bot_gazebo/share/bot_gazebo/models
    # GAZEBO PLUGINS PATH==/usr/share/gazebo/../../lib/x86_64-linux-gnu/gazebo-11/plugins::/home/wyj/myself/1_code/3_ros2_about/two_wheeled_car/install/bot_gazebo/share/bot_gazebo/plugins:/home/wyj/myself/1_code/3_ros2_about/two_wheeled_car/install/bot_description/lib


    # Gazebo 启动
    gazebo = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(pkg_gazebo_ros, 'launch', 'gazebo.launch.py'),
        )
    )

    # 返回包含启动参数和 Gazebo 启动的 launch 描述
    return LaunchDescription([
        DeclareLaunchArgument(
            'world',
            default_value=[os.path.join(
                pkg_bot_gazebo, 'worlds', 'dynamic.world'), ''],
            description='SDF world 文件'),
        gazebo
    ])
